#include "ODECustomMesh.h"
#include "ODECommon.h"
#include <ode/collision.h>
#include <Timer.h>
#include <errors.h>
using namespace std;

const static Real cptol=1e-5;

int gdCustomMeshClass = 0;

dGeomID dCreateCustomMesh(CollisionMesh* mesh,Real outerMargin)
{
  dGeomID geom = dCreateGeom(gdCustomMeshClass);
  CustomMeshData* data = dGetCustomMeshData(geom);
  data->mesh = mesh;
  data->outerMargin = outerMargin;
  dGeomSetCategoryBits(geom,0xffffffff);
  dGeomSetCollideBits(geom,0xffffffff);
  dGeomEnable(geom);
  return geom;
}

CustomMeshData* dGetCustomMeshData(dGeomID o)
{
  return (CustomMeshData*)dGeomGetClassData(o);
}

//1 = pt, 2 = edge, 3 = face
inline int FeatureType(const Vector3& b) 
{
  int type=0;
  if(FuzzyZero(b.x)) type++;
  if(FuzzyZero(b.y)) type++;
  if(FuzzyZero(b.z)) type++;
  return 3-type;
}

int EdgeIndex(const Vector3& b)
{
  if(FuzzyZero(b.x)) return 0;
  if(FuzzyZero(b.y)) return 1;
  if(FuzzyZero(b.z)) return 2;
  FatalError("Shouldn't get here");
  return -1;
}

int VertexIndex(const Vector3& b)
{
  if(FuzzyEquals(b.x,One)) return 0;
  if(FuzzyEquals(b.y,One)) return 1;
  if(FuzzyEquals(b.z,One)) return 2;
  FatalError("Shouldn't get here");
  return -1;
}

Vector3 VertexNormal(const CollisionMesh& m,int tri,int vnum)
{
  Assert(!m.incidentTris.empty());
  int v=m.tris[tri][vnum];
  Vector3 n(Zero);
  for(size_t i=0;i<m.incidentTris[v].size();i++)
    n += m.TriangleNormal(m.incidentTris[v][i]);
  n.inplaceNormalize();
  return n;
}

Vector3 EdgeNormal(const CollisionMesh& m,int tri,int e)
{
  Assert(!m.triNeighbors.empty());
  Vector3 n=m.TriangleNormal(tri);
  n += m.TriangleNormal(m.triNeighbors[tri][e]);
  n.inplaceNormalize();
  return n;
}

//returns the normal needed for m1 to get out of m2
Vector3 ContactNormal(const CollisionMesh& m1,const CollisionMesh& m2,const Vector3& p1,const Vector3& p2,int t1,int t2)
{
  Triangle3D tri1,tri2;
  m1.GetTriangle(t1,tri1);
  m2.GetTriangle(t2,tri2);
  Vector3 b1=tri1.barycentricCoords(p1);
  Vector3 b2=tri2.barycentricCoords(p2);
  int type1=FeatureType(b1),type2=FeatureType(b2);
  switch(type1) {
  case 1:  //pt
    switch(type2) {
    case 1:  //pt
      //get the triangle normals
      {
	Vector3 n1 = VertexNormal(m1,t1,VertexIndex(b1));
	Vector3 n2 = VertexNormal(m2,t2,VertexIndex(b2));
	n2 -= n1;
	n2.inplaceNormalize();
	return n2;
      }
      break;
    case 2:  //edge
      {
	Vector3 n1 = VertexNormal(m1,t1,VertexIndex(b1));
	int e = EdgeIndex(b2);
	Segment3D s = tri2.edge(e);
	Vector3 ev = s.b-s.a;
	Vector3 n2 = EdgeNormal(m2,t2,e);
	n2-=(n1-ev*ev.dot(n1)/ev.dot(ev)); //project onto normal
	n2.inplaceNormalize();
	return n2;
      }
      break;
    case 3:  //face
      return m2.currentTransform.R*tri2.normal();
    }
    break;
  case 2:  //edge
    switch(type2) {
    case 1:  //pt
      {
	Vector3 n2 = VertexNormal(m2,t2,VertexIndex(b2));
	int e = EdgeIndex(b1);
	Segment3D s = tri1.edge(e);
	Vector3 ev = s.b-s.a;
	Vector3 n1 = EdgeNormal(m1,t1,e);
	n2 = (n2-ev*ev.dot(n2)/ev.dot(ev))-n1; //project onto normal
	n2.inplaceNormalize();
	return n2;
      }
      break;
    case 2:  //edge
      {
	int e = EdgeIndex(b1);
	Vector3 n1 = EdgeNormal(m1,t1,e);
	e = EdgeIndex(b2);
	Vector3 n2 = EdgeNormal(m2,t2,e);
	n2 -= n1;
	n2.inplaceNormalize();
	return n2;
      }
      break;
    case 3:  //face
      return m2.currentTransform.R*tri2.normal();
    }
    break;
  case 3:  //face
    return m1.currentTransform.R*(-tri1.normal());
  }
  AssertNotReached();
  return Vector3(Zero);
}

int dCustomMeshCollide (dGeomID o1, dGeomID o2, int flags,
			   dContactGeom *contact, int skip)
{
  int m = (flags&0xffff);
  if(m == 0) m=1;
  //printf("CustomMesh collide\n");
  CustomMeshData* d1 = dGetCustomMeshData(o1);
  CustomMeshData* d2 = dGetCustomMeshData(o2);
  CopyMatrix(d1->mesh->currentTransform.R,dGeomGetRotation(o1));
  CopyVector(d1->mesh->currentTransform.t,dGeomGetPosition(o1));
  //cout<<"Transform 1:"<<endl<<d1->mesh->currentTransform<<endl;
  CopyMatrix(d2->mesh->currentTransform.R,dGeomGetRotation(o2));
  CopyVector(d2->mesh->currentTransform.t,dGeomGetPosition(o2));
  //cout<<"Transform 2:"<<endl<<d2->mesh->currentTransform<<endl;
  CollisionMeshQuery q(*d1->mesh,*d2->mesh);
  bool res=q.WithinDistanceAll(d1->outerMargin+d2->outerMargin);
  if(!res) return 0;

  vector<int> t1,t2;
  vector<Vector3> cp1,cp2;
  q.TolerancePairs(t1,t2);
  q.TolerancePoints(cp1,cp2);
  //printf("%d Collision pairs\n",t1.size());
  const RigidTransform& T1=d1->mesh->currentTransform;
  const RigidTransform& T2=d2->mesh->currentTransform;
  RigidTransform T21; T21.mulInverseA(T1,T2);
  RigidTransform T12; T12.mulInverseA(T2,T1);
  Real tol = d1->outerMargin+d2->outerMargin;
  Real tol2 = Sqr(d1->outerMargin+d2->outerMargin);

  //test if more triangle vertices are closer than tolerance
  /*
  size_t imax=t1.size();
  Triangle3D tri1,tri2,tri1loc,tri2loc;
  for(size_t i=0;i<imax;i++) {
    d1->mesh->GetTriangle(t1[i],tri1);
    d2->mesh->GetTriangle(t2[i],tri2);

    tri1loc.a = T12*tri1.a;
    tri1loc.b = T12*tri1.b;
    tri1loc.c = T12*tri1.c;
    tri2loc.a = T21*tri2.a;
    tri2loc.b = T21*tri2.b;
    tri2loc.c = T21*tri2.c;
    Vector3 cpa = tri1.closestPoint(tri2loc.a);
    if(cpa.distanceSquared(tri2loc.a) < tol2) {
      if(!cpa.isEqual(cp1[i],cptol) && !tri2.a.isEqual(cp2[i],cptol)) {
	t1.push_back(t1[i]);
	t2.push_back(t2[i]);
	cp1.push_back(cpa);
	cp2.push_back(tri2.a);
      }
    }
    Vector3 cpb = tri1.closestPoint(tri2loc.b);
    if(cpb.distanceSquared(tri2loc.b) < tol2) {
      if(!cpb.isEqual(cp1[i],cptol) && !tri2.b.isEqual(cp2[i],cptol)) {
	t1.push_back(t1[i]);
	t2.push_back(t2[i]);
	cp1.push_back(cpb);
	cp2.push_back(tri2.b);
      }
    }
    Vector3 cpc = tri1.closestPoint(tri2loc.c);
    if(cpc.distanceSquared(tri2loc.c) < tol2) {
      if(!cpc.isEqual(cp1[i],cptol) && !tri2.c.isEqual(cp2[i],cptol)) {
	t1.push_back(t1[i]);
	t2.push_back(t2[i]);
	cp1.push_back(cpc);
	cp2.push_back(tri2.c);
      }
    }
    Vector3 cpa2 = tri2.closestPoint(tri1loc.a);
    if(cpa2.distanceSquared(tri1loc.a) < tol2) {
      if(!cpa2.isEqual(cp2[i],cptol) && !tri1.a.isEqual(cp1[i],cptol)) {
	t1.push_back(t1[i]);
	t2.push_back(t2[i]);
	cp1.push_back(tri1.a);
	cp2.push_back(cpa2);
      }
    }
    Vector3 cpb2 = tri2.closestPoint(tri1loc.b);
    if(cpb2.distanceSquared(tri1loc.b) < tol2) {
      if(!cpb2.isEqual(cp2[i],cptol) && !tri1.b.isEqual(cp1[i],cptol)) {
	t1.push_back(t1[i]);
	t2.push_back(t2[i]);
	cp1.push_back(tri1.b);
	cp2.push_back(cpb2);
      }
    }
    Vector3 cpc2 = tri2.closestPoint(tri1loc.c);
    if(cpc2.distanceSquared(tri1loc.c) < tol2) {
      if(!cpc2.isEqual(cp2[i],cptol) && !tri1.c.isEqual(cp1[i],cptol)) {
	t1.push_back(t1[i]);
	t2.push_back(t2[i]);
	cp1.push_back(tri1.c);
	cp2.push_back(cpc2);
      }
    }
  }
  */
  //if(t1.size() != imax)
  //printf("Triangle vert checking added %d points\n",t1.size()-imax);
  //getchar();

  size_t imax=t1.size();
  Triangle3D tri1,tri2,tri1loc;
  for(size_t i=0;i<imax;i++) {
    d1->mesh->GetTriangle(t1[i],tri1);
    d2->mesh->GetTriangle(t2[i],tri2);

    tri1loc.a = T12*tri1.a;
    tri1loc.b = T12*tri1.b;
    tri1loc.c = T12*tri1.c;
    if(tri1loc.intersects(tri2)) { 
      printf("Intersect tri -- margin %g -- deleting contact pair %d\n",d1->outerMargin+d2->outerMargin,i);
      //the two triangles intersect! can't trust results of PQP
      t1[i] = t1.back();
      t2[i] = t2.back();
      cp1[i] = cp1.back();
      cp2[i] = cp2.back();
      i--;
      imax--;
    }
  }
  if(t1.size() != imax) {
    printf("Triangle collision checking deleted %d points\n",t1.size()-imax);
    t1.resize(imax);
    t2.resize(imax);
    cp1.resize(imax);
    cp2.resize(imax);
  }
  
  int k=0;  //count the # of contact points added
  for(size_t i=0;i<cp1.size();i++) {
    Vector3 p1 = T1*cp1[i];
    Vector3 p2 = T2*cp2[i];
    Vector3 n=p1-p2;
    Real d = n.norm();
    if(d < 1e-5) {  //compute normal from the geometry
      n = ContactNormal(*d1->mesh,*d2->mesh,cp1[i],cp2[i],t1[i],t2[i]);
    }
    else if(d > tol) {  //some penetration -- we can't trust the result of PQP
      continue;
    }
    else n /= d;
    //cout<<"Local Points "<<cp1[i]<<", "<<cp2[i]<<endl;
    //cout<<"Points "<<p1<<", "<<p2<<endl;
    CopyVector(contact[k].pos,(p1+p2)*0.5);
    CopyVector(contact[k].normal,n);
    contact[k].depth = tol - d;
    if(contact[k].depth < 0) contact[k].depth = 0;
    //cout<<"Normal "<<n<<", depth "<<contact[i].depth<<endl;
    //getchar();
    contact[k].g1 = o1;
    contact[k].g2 = o2;
    k++;
    if(k == m) break;
  }
  return k;
}

/*
int dCustomMeshTriMeshCollide (dGeomID o1, dGeomID o2, int flags,
				  dContactGeom *contact, int skip)
{
  int n = (flags&0xffff);
  if(n == 0) n=1;  
}
*/

dColliderFn * dCustomMeshGetColliderFn (int num)
{
  if(num == gdCustomMeshClass) return dCustomMeshCollide;
  //else if(num == dTriMeshClass) return dCustomMeshTriMeshCollide;
  else return NULL;
}

void dCustomMeshAABB(dGeomID o,dReal aabb[6])
{
  CustomMeshData* d = dGetCustomMeshData(o);
  Box3D box;
  AABB3D bb;
  CopyMatrix(d->mesh->currentTransform.R,dGeomGetRotation(o));
  CopyVector(d->mesh->currentTransform.t,dGeomGetPosition(o));  
  GetBB(*d->mesh,box);
  box.getAABB(bb);
  CopyVector3(&aabb[0],bb.bmin);
  CopyVector3(&aabb[3],bb.bmax);
}

void dCustomMeshDtor(dGeomID o)
{
}

void InitODECustomMesh()
{
  dGeomClass mmclass;
  mmclass.bytes = sizeof(CustomMeshData);
  mmclass.collider = dCustomMeshGetColliderFn;
  //mmclass.aabb = dCustomMeshAABB;
  mmclass.aabb = dInfiniteAABB;
  mmclass.aabb_test = NULL;
  mmclass.dtor = dCustomMeshDtor;
  gdCustomMeshClass = dCreateGeomClass(&mmclass);
}

